#include <ros/ros.h>
#include "robot_msgs/EventStatus.h"

int main(int argc, char** argv)
{
  ros::init(argc, argv, "event_status_pub_test");
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<robot_msgs::EventStatus>("/sentry/event_status", 10);

  robot_msgs::EventStatus msg;
  msg.self_health = 90.0;
  msg.enemy_outpost_health = 1500.0;
  msg.own_outpost_destroyed = false;

  msg.buff_active = false;
  msg.enemy_detected.is_get = false;
  msg.enemy_detected.position.x = 0.1;
  msg.enemy_detected.position.y = 0.3;
  msg.enemy_detected.position.z = 8.0;
  msg.enemy_detected.armor_id = 3;

  ros::Rate rate(1);  // 1Hz
  int count = 0;
  while(ros::ok())
  {
    // if(count == 15)
    // {
    //   msg.self_health = 100.0;
    // }
    // else if(count == 30)
    // {
    //   msg.enemy_detected.is_get = true;
    // }
    // else if(count == 45)
    // {
    //   msg.self_health = 350.0;
    // }
    // else if(count == 60)
    // {
    //   msg.enemy_outpost_health = 0.0;
    // }
    // else if(count == 90)
    // {
    //   msg.enemy_detected.is_get = false;
    // }
    // else if(count == 105)
    // {
    //   msg.self_health = 110.0;
    // }
    // else if(count == 120)
    // {
    //   msg.self_health = 350.0;
    // }
    pub.publish(msg);
    std::cout << "health: " << msg.self_health << ", buff: " << msg.buff_active
              << ", enemy: " << (msg.enemy_detected.is_get ? "true" : "false")
              << ", outpost: " << (msg.own_outpost_destroyed ? "true" : "false")
              << ", enemy_outpost_health: " << msg.enemy_outpost_health << std::endl;
    count += 5;
    ROS_INFO("Published test EventStatus");
    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}